/*
 * Copyright (C) 2021-2022,2023 NXP Semiconductors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <uwb_uwbs_tml_interface.h>
#include "phUwbTypes.h"

#include "phOsalUwb.h"
#include "phNxpUwb_Common.h"
#include "phUwb_BuildConfig.h"
#include "phNxpLogApis_TmlUwb.h"
#include "phUwbErrorCodes.h"
#include "uwb_board.h"

/** Local functions prototypes */
static UWBStatus_t spi_assert_cs(uwb_uwbs_tml_ctx_t *pCtx);
static UWBStatus_t spi_deassert_cs(uwb_uwbs_tml_ctx_t *pCtx);
static UWBStatus_t wait_helios_rdy_high(uwb_uwbs_tml_ctx_t *pCtx);
static UWBStatus_t wait_helios_rdy_low(uwb_uwbs_tml_ctx_t *pCtx);
static UWBStatus_t check_helios_irq_low(uwb_uwbs_tml_ctx_t *pCtx, uint16_t *count);
static UWBStatus_t check_helios_irq_high(uwb_uwbs_tml_ctx_t *pCtx, uint16_t *count);
static UWBStatus_t uwb_rci_write(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen);
static UWBStatus_t uwb_rci_read(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen);
static UWBStatus_t uwb_uci_write(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen);
static UWBStatus_t uwb_uci_read(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen);

#if UWBIOT_UWBD_SR040

#define PAYLOAD_LEN_MSB 0x02
#define PAYLOAD_LEN_LSB 0x03

#define TIME_WAIT_MS                  100
#define TIME_WAIT_AFTER_OPERATION     200
#define TIME_WAIT_AFTER_CS_READ       10
#define TIME_WAIT_AFTER_CS_WRITE      10
#define TIMEOUT_DELAY                 5
#define WAIT_FOR_READY_AT_WRITE_COUNT 400

#define RCI_PKT_LEN                     256
#define UCI_HDR_LEN                     0x04
#define NORMAL_MODE_LEN_OFFSET          0x03
#define EXTND_LEN_INDICATOR_OFFSET      0x01
#define EXTND_LEN_INDICATOR_OFFSET_MASK 0x80
#define EXTENDED_LENGTH_OFFSET          0x02

static UWBStatus_t spi_assert_cs(uwb_uwbs_tml_ctx_t *pCtx)
{
    /* assert CS. */
    if (uwb_bus_io_val_set(&pCtx->busCtx, kUWBS_IO_O_UWBS_CS, kUWBS_IO_State_Low) != kUWB_bus_Status_OK) {
        return kUWBSTATUS_FAILED;
    }
    /* After CS Asserted */
    /* Add minimum delay for CS assert to get refeclted */
    phOsalUwb_Delay(1);
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t spi_deassert_cs(uwb_uwbs_tml_ctx_t *pCtx)
{
    /* deassert CS. */
    if (uwb_bus_io_val_set(&pCtx->busCtx, kUWBS_IO_O_UWBS_CS, kUWBS_IO_State_High) != kUWB_bus_Status_OK) {
        return kUWBSTATUS_FAILED;
    }
    /* After CS DeAsserted */
    /* Add minimum delay for CS Deassert to get refeclted */
    phOsalUwb_Delay(1);
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t wait_helios_rdy_high(uwb_uwbs_tml_ctx_t *pCtx)
{
    uwb_bus_status_t bus_status;
    uint16_t count            = 0;
    uwbs_io_state_t gpioValue = kUWBS_IO_State_NA;

    phNxpUwb_BackoffDelayReset(&count);
    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("wait_helios_rdy_high : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
        return kUWBSTATUS_FAILED;
    }

    while (gpioValue == kUWBS_IO_State_Low) {
        if (phNxpUwb_BackoffDelay(&count) == 1) {
            LOG_D("SPI Backoff timeout. Line: %d", __LINE__);
            return kUWBSTATUS_FAILED;
        }
        bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
        if (bus_status != kUWB_bus_Status_OK) {
            LOG_E("wait_helios_rdy_high : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
            return kUWBSTATUS_FAILED;
        }
    }
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t wait_helios_rdy_low(uwb_uwbs_tml_ctx_t *pCtx)
{
    uwb_bus_status_t bus_status;
    uint16_t count            = 0;
    uwbs_io_state_t gpioValue = kUWBS_IO_State_NA;

    phNxpUwb_BackoffDelayReset(&count);
    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("wait_helios_rdy_low : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
        return kUWBSTATUS_FAILED;
    }

    while (gpioValue == kUWBS_IO_State_High) {
        if (phNxpUwb_BackoffDelay(&count) == 1) {
            LOG_D("SPI Backoff timeout. Line: %d", __LINE__);
            return kUWBSTATUS_FAILED;
        }
        bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
        if (bus_status != kUWB_bus_Status_OK) {
            LOG_E("wait_helios_rdy_low : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
            return kUWBSTATUS_FAILED;
        }
    }
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t check_helios_irq_low(uwb_uwbs_tml_ctx_t *pCtx, uint16_t *count)
{
    uwb_bus_status_t bus_status;
    uwbs_io_state_t gpioValue = kUWBS_IO_State_NA;

    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("check_helios_irq_low : gpio get failed kUWBS_IO_I_UWBS_IRQ");
        return kUWBSTATUS_FAILED;
    }

    if (gpioValue == kUWBS_IO_State_Low) {
        if (phNxpUwb_BackoffDelay(count) == 1) {
            LOG_D("check_helios_irq_low : SPI Backoff timeout. Line: %d", __LINE__);
            return kUWBSTATUS_FAILED;
        }
        return kUWBSTATUS_RETRY;
    }
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t check_helios_irq_high(uwb_uwbs_tml_ctx_t *pCtx, uint16_t *count)
{
    uwb_bus_status_t bus_status;
    uwbs_io_state_t gpioValue = kUWBS_IO_State_NA;

    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("check_helios_irq_high : gpio get failed kUWBS_IO_I_UWBS_IRQ");
        return kUWBSTATUS_FAILED;
    }

    if (gpioValue == kUWBS_IO_State_High) {
        if (phNxpUwb_BackoffDelay(count) == 1) {
            LOG_D("check_helios_irq_high : SPI Backoff timeout. Line: %d", __LINE__);
            return kUWBSTATUS_FAILED;
        }
        return kUWBSTATUS_RETRY;
    }
    return kUWBSTATUS_SUCCESS;
}

static UWBStatus_t uwb_rci_write(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen)
{
    uwb_bus_status_t bus_status;
    UWBStatus_t status = kUWBSTATUS_FAILED;

    uint8_t rci_write_buffer[RCI_PKT_LEN] = {0};

    if (bufLen > RCI_PKT_LEN) {
        return status;
    }

    phOsalUwb_LockMutex(pCtx->mSyncMutex);
    phOsalUwb_MemCopy(rci_write_buffer, pBuf, bufLen);

    bus_status = uwb_bus_data_crc16_xmodem(&pCtx->busCtx, rci_write_buffer, RCI_PKT_LEN - 2);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_rci_write : uwb_bus_data_crc16_xmodem failed");
        goto end;
    }
    rci_write_buffer[RCI_PKT_LEN - 2] = ((pCtx->busCtx.crc >> (8 * 0)) & 0xFF);
    rci_write_buffer[RCI_PKT_LEN - 1] = ((pCtx->busCtx.crc >> (8 * 1)) & 0xFF);

    /* RDYn line needs to be high before selecting the chip. SR040 will use this line to signal to
     * HostS that it is ready to receive new command, after selected by CSn line. Implemented
     * handshaking follows following scheme:
     * 1. RDYn is high       <- Required initial state
     * 2. Set CSn low        <- Select SR040 device
     * 3. Wait RDYn gets low <- SR040 ready to receive
     * 4. Send command
     * 5. Set CSn high       <- SR040 no longer selected, but RDYn can still be low
     * 6. RDYn gets high */

    /*wait for ready pin is high */
    status = wait_helios_rdy_high(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto end;
    }

    status = spi_assert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_rci_write : spi_assert_cs failed");
        goto end;
    }

    /*wait for ready pin to go low*/
    status = wait_helios_rdy_low(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto end;
    }

    bus_status = uwb_bus_data_tx(&pCtx->busCtx, rci_write_buffer, RCI_PKT_LEN);
    if (bus_status != kUWB_bus_Status_OK) {
        status = kUWBSTATUS_FAILED;
        (void)spi_deassert_cs(pCtx);
        pCtx->noOfBytesWritten = -1;
        goto end;
    }
    pCtx->noOfBytesWritten = RCI_PKT_LEN;

    status = spi_deassert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_rci_write : spi_deassert_cs failed");
        goto end;
    }

    phOsalUwb_Delay(TIME_WAIT_AFTER_CS_WRITE);

    status = wait_helios_rdy_high(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto end;
    }
end:
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
    return status;
}

static UWBStatus_t uwb_rci_read(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen)
{
    uwb_bus_status_t bus_status;
    UWBStatus_t status                   = kUWBSTATUS_FAILED;
    uint16_t payloadLen                  = UCI_HDR_LEN;
    uint8_t rci_read_buffer[RCI_PKT_LEN] = {0};
    uint8_t *bufPtr                      = &rci_read_buffer[0];
    uint8_t count                        = 0;
    uwbs_io_state_t gpioValue            = kUWBS_IO_State_NA;

    /* INTn line needs to be LOW before selecting the chip. SR040 will use this line to signal to
     * HostS that data is available to read, after selected by CSn line. Implemented
     * handshaking follows following scheme:
     * 1. INTn is Low & RDYn is high       <- Required initial state
     * 2. Set CSn low        <- Select SR040 device
     * 3. INTn goes High
     * 4. Wait RDYn gets low <- SR040 ready to send response
     * 5. Read Response
     * 6. Set CSn high       <- SR040 no longer selected, but RDYn can still be low
     * 7. RDYn gets high */

    phOsalUwb_LockMutex(pCtx->mSyncMutex);

    uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    while (gpioValue) {
        uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
        count++;
        if (count > TIMEOUT_DELAY) {
            goto end;
        }
    }

    status = spi_assert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_rci_read : spi_assert_cs failed");
        goto end;
    }

    status     = kUWBSTATUS_FAILED;
    bus_status = uwb_bus_data_rx(&pCtx->busCtx, rci_read_buffer, sizeof(rci_read_buffer));
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_rci_read : uwb_bus_data_rx failed");
        goto end;
    }

    status = spi_deassert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_rci_read : spi_assert_cs failed");
        goto end;
    }

    phOsalUwb_Delay(TIME_WAIT_AFTER_CS_READ);

    if (rci_read_buffer[0] == 0x00) {
        bufPtr++;
        payloadLen = payloadLen + rci_read_buffer[4];
    }
    else {
        payloadLen = payloadLen + rci_read_buffer[3];
    }

    status = wait_helios_rdy_high(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto end;
    }

    bus_status = uwb_bus_data_crc16_xmodem(&pCtx->busCtx, rci_read_buffer, RCI_PKT_LEN - 2);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_rci_read : uwb_bus_data_crc16_xmodem failed");
        status = kUWBSTATUS_FAILED;
        goto end;
    }
    if (((pCtx->busCtx.crc & 0xFF) != rci_read_buffer[RCI_PKT_LEN - 2]) ||
        (((pCtx->busCtx.crc >> 8) & 0xFF) != rci_read_buffer[RCI_PKT_LEN - 1])) {
        status = kUWBSTATUS_FAILED;
        goto end;
    }

    *pBufLen = payloadLen;
    phOsalUwb_MemCopy(pBuf, bufPtr, payloadLen);

end:
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
    return status;
}

static UWBStatus_t uwb_uci_write(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen)
{
    uwb_bus_status_t bus_status;
    UWBStatus_t status;
    pCtx->noOfBytesWritten                   = -1;
    uwbs_io_state_t gpioValue                = kUWBS_IO_State_NA;
    uint16_t retryCount                      = 0;
    const uint16_t MAX_UCI_WRITE_RETRY_LIMIT = 10;
    uint16_t count                           = 0;
    uint16_t timeout                         = 0;

    phNxpUwb_BackoffDelayReset(&retryCount);

start:
    phOsalUwb_LockMutex(pCtx->mSyncMutex);

    /* RDYn line needs to be high before selecting the chip. SR040 will use this line to signal to
     * HostS that it is ready to receive new command, after selected by CSn line. Implemented
     * handshaking follows following sheme:
     * 0. IRQ is low         <- No Pending read
     * 1. RDYn is high       <- Required initial state
     * 2. Set CSn low        <- Select SR040 device
     * 3. Wait RDYn gets low <- SR040 ready to receive
     * 4. Send command
     * 5. Set CSn high       <- SR040 no longer selected
     * 6. wait till RDYn pin goes high */

    /* Check for INT_N, it should not be low before write operation */

    status = check_helios_irq_low(pCtx, &retryCount);
    if (status == kUWBSTATUS_RETRY) {
        phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
        ENSURE_BACKOFF_TIMEOUT_OR_CLEANUP(count); /* Let other task get control */
        count += 15;
        goto start;
    }
    else if (status == kUWBSTATUS_FAILED) {
        /* Timed out */
        LOG_W("Read may be pending.");
        status = kUWBSTATUS_BUSY;
        goto unlockmutex;
    }

    /* Initial condition: RDY pin must be HIGH */
    phNxpUwb_BackoffDelayReset(&retryCount);
    status = wait_helios_rdy_high(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto unlockmutex;
    }

    /* assert CS. */
    status = spi_assert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_uci_write : spi_assert_cs failed");
        goto unlockmutex;
    }

    status     = kUWBSTATUS_FAILED;
    retryCount = 0;

    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_uci_write : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
        goto unlockmutex;
    }

    while ((kUWBS_IO_State_High == gpioValue) && (retryCount++ < MAX_UCI_WRITE_RETRY_LIMIT)) {
        __WFI();
        bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_READY_IRQ, &gpioValue);
        if (bus_status != kUWB_bus_Status_OK) {
            LOG_E("uwb_uci_write : gpio get failed kUWBS_IO_I_UWBS_READY_IRQ");
            goto unlockmutex;
        }
        phOsalUwb_Delay(1);
    }
    /*
     check if INT_n is low
     */
    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_uci_write : gpio get failed kUWBS_IO_I_UWBS_IRQ");
        goto unlockmutex;
    }
    if ((gpioValue == kUWBS_IO_State_Low) || (retryCount >= MAX_UCI_WRITE_RETRY_LIMIT)) {
        goto unlockmutex;
    }

    /* RDY pin asserted, write data */
    bus_status = uwb_bus_data_tx(&pCtx->busCtx, pBuf, bufLen);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("uwb_uci_write : SPI write failed");
        goto unlockmutex;
    }

    /* De-assert CS. RDY pin must go HIGH */
    status = spi_deassert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_uci_write : gpio set failed kUWBS_IO_O_UWBS_CS");
        goto unlockmutex;
    }

    /* Wait till ready pin goes high */
    status = wait_helios_rdy_high(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        goto unlockmutex;
    }

    pCtx->noOfBytesWritten = bufLen;
unlockmutex:
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);

cleanup:
    return status;
}

static UWBStatus_t uwb_uci_read(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen)
{
    uwb_bus_status_t bus_status;
    UWBStatus_t status;
    size_t payloadLen;
    size_t totalBtyesToRead            = 0;
    uint32_t IsExtndLenIndication      = 0;
    uint8_t dummyBuff[UCI_HDR_LEN + 1] = {0};
    uint16_t count                     = 0;
    uint16_t timeout                   = 0;
    uwbs_io_state_t gpioValue          = kUWBS_IO_State_NA;

    /* INTn line needs to be LOW before selecting the chip. SR040 will use this line to signal to
     * HostS that data is available to read, after selected by CSn line. Implemented
     * handshaking follows following scheme:
     * 1. INTn is Low        <- Required initial state
     * 2. Set CSn low        <- Select SR040 device
     * 3. Read Response
     * 4. wait till INTn pin goes high
     * 5. Set CSn high       <- SR040 no longer selected*/

    phNxpUwb_BackoffDelayReset(&count);

start:

    bus_status = uwb_bus_io_irq_wait(&pCtx->busCtx, MAX_DELAY);
    if (bus_status == kUWB_bus_Status_FAILED) {
        uwb_bus_io_irq_dis(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ);
        ENSURE_BACKOFF_TIMEOUT_OR_RET(count, kUWBSTATUS_RESPONSE_TIMEOUT);
        goto start;
    }

    phOsalUwb_LockMutex(pCtx->mSyncMutex);

    /* assert CS. */
    status = spi_assert_cs(pCtx);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_uci_read : spi_assert_cs failed");
        goto unlockmutex;
    }

    /* INT_n should remain asserted. If not, it could be a spurious
     * interrupt or the device may have gone in to low power mode
     */
    status = check_helios_irq_high(pCtx, &count);
    if (status != kUWBSTATUS_SUCCESS) {
        if (spi_deassert_cs(pCtx) != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uci_read : spi_deassert_cs failed");
            goto unlockmutex;
        }
        if (status == kUWBSTATUS_RETRY) {
            phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
            goto start;
        }
        else {
            status = kUWBSTATUS_FAILED;
            goto unlockmutex;
        }
    }

    status = kUWBSTATUS_FAILED;

    /* Read UCI header */
    bus_status = uwb_bus_data_rx(&pCtx->busCtx, dummyBuff, UCI_HDR_LEN + 1);
    if (bus_status == kUWB_bus_Status_OK) {
        if (dummyBuff[0] == 0x00) {
            if (*pBufLen >= UCI_HDR_LEN) {
                /* First byte read was 0x00, UCI header starts from second byte */
                phOsalUwb_MemCopy(&pBuf[0], &dummyBuff[1], UCI_HDR_LEN);
            }
            else {
                LOG_E("%s Not enough buffer", __FUNCTION__);
                goto waitforHighandExit;
            }
        }
        else if (dummyBuff[0] == 0xFF) {
            /* Read failed. The device may have gone in to low power mode.
             * Return failure from here.
             */
            goto waitforHighandExit;
        }
        else {
            if (*pBufLen >= UCI_HDR_LEN) {
                phOsalUwb_MemCopy(&pBuf[0], &dummyBuff[0], UCI_HDR_LEN);
            }
            else {
                LOG_E("%s Not enough buffer", __FUNCTION__);
                goto waitforHighandExit;
            }
        }
    }
    else {
        LOG_E("uwb_uci_read : SPI read failed");
        goto waitforHighandExit;
    }

    if (pBuf[0] == 0x00) {
        /* Empty packet. Backoff and retry read */
        status = spi_deassert_cs(pCtx);
        if (status != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uci_read : spi_deassert_cs failed");
            goto waitforHighandExit;
        }
        phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
        ENSURE_BACKOFF_TIMEOUT_OR_RET(count, kUWBSTATUS_RESPONSE_TIMEOUT);
        goto start;
    }

    IsExtndLenIndication = (pBuf[EXTND_LEN_INDICATOR_OFFSET] & EXTND_LEN_INDICATOR_OFFSET_MASK);

    totalBtyesToRead = pBuf[NORMAL_MODE_LEN_OFFSET];

    if (IsExtndLenIndication) {
        totalBtyesToRead = (uint16_t)((totalBtyesToRead << 8) | pBuf[EXTENDED_LENGTH_OFFSET]);
    }

    payloadLen = totalBtyesToRead;

    /* Read payload */
    if (payloadLen != 0) {
        if (*pBufLen >= (UCI_HDR_LEN + payloadLen)) {
            bus_status = uwb_bus_data_rx(&pCtx->busCtx, &pBuf[UCI_HDR_LEN], payloadLen);
            if (bus_status != kUWB_bus_Status_OK) {
                LOG_E("uwb_uci_read : SPI read failed");
                *pBufLen = 0;
                goto waitforHighandExit;
            }
        }
        else {
            *pBufLen = 0;
            LOG_E("%s Not enough buffer, %d bytes of buffer is needed", __FUNCTION__, (UCI_HDR_LEN + payloadLen));
            goto waitforHighandExit;
        }
    }
    *pBufLen = (UCI_HDR_LEN + payloadLen);

waitforHighandExit:
    /* INT pin must go HIGH after data is read out */
    phNxpUwb_BackoffDelayReset(&count);
    bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    do {
        /*wait till INTn pin goes High*/
        ENSURE_BACKOFF_TIMEOUT_OR_CLEANUP(count);
        bus_status = uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
    } while (gpioValue == kUWBS_IO_State_Low);

    status = kUWBSTATUS_SUCCESS;
cleanup:
    (void)spi_deassert_cs(pCtx);

unlockmutex:
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
    return status;
}

UWBStatus_t uwb_uwbs_tml_init(uwb_uwbs_tml_ctx_t *pCtx)
{
    UWBStatus_t status;
    uwb_bus_status_t bus_status;

    if (pCtx == NULL) {
        LOG_E("uwbs tml context is NULL");
        status = kUWBSTATUS_INVALID_PARAMETER;
        goto exit;
    }

    phOsalUwb_SetMemory(pCtx, 0, sizeof(uwb_uwbs_tml_ctx_t));

    // by default set tml mode to UCI
    pCtx->mode = kUWB_UWBS_TML_MODE_UCI;

    status = (UWBStatus_t)phOsalUwb_CreateMutex(&(pCtx->mSyncMutex));
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("Error: uwb_uwbs_tml_init(), could not create mutex mSyncMutex\n");
        goto exit;
    }

    status = (UWBStatus_t)phOsalUwb_CreateSemaphore(&(pCtx->mReadyIrqWaitSem), 0);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("Error: uwb_uwbs_tml_init(), could not create semaphore mReadyIrqWaitSem\n");
        goto exit;
    }

    bus_status = uwb_bus_init(&pCtx->busCtx);
    if (bus_status != kUWB_bus_Status_OK) {
        status = kUWBSTATUS_CONNECTION_FAILED;
        LOG_E("Error: uwb_uwbs_tml_init(), uwb bus initialisation failed");
    }
#if ((defined(UWBIOT_HOST_S32K144_SR040)) && (UWBIOT_HOST_S32K144_SR040 != 0))
    uwb_bus_data_crc16_xmodem_init(&pCtx->busCtx);
#endif // UWBIOT_HOST_S32K144_SR040
exit:
    return status;
}

UWBStatus_t uwb_uwbs_tml_setmode(uwb_uwbs_tml_ctx_t *pCtx, uwb_uwbs_tml_mode_t mode)
{
    if (pCtx == NULL) {
        LOG_E("uwbs tml context is NULL");
        return kUWBSTATUS_INVALID_PARAMETER;
    }
    pCtx->mode = mode;
    return kUWBSTATUS_SUCCESS;
}

UWBStatus_t uwb_uwbs_tml_deinit(uwb_uwbs_tml_ctx_t *pCtx)
{
    UWBStatus_t status;
    uwb_bus_status_t bus_status;

    status = (UWBStatus_t)phOsalUwb_DeleteMutex(&(pCtx->mSyncMutex));
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("Error: uwb_uwbs_tml_deinit(), could not delete mutex mSyncMutex\n");
    }

    if (UWBSTATUS_SUCCESS != phOsalUwb_ProduceSemaphore(pCtx->mReadyIrqWaitSem)) {
        LOG_D("Error: UWB_HeliosInit(), could not post semaphore mReadyIrqWaitSem\n");
    }

    phOsalUwb_DeleteSemaphore(&pCtx->mReadyIrqWaitSem);

    bus_status = uwb_bus_deinit(&pCtx->busCtx);
    if (bus_status != kUWB_bus_Status_OK) {
        LOG_E("Error: uwb_uwbs_tml_deinit(), uwb bus de init failed");
        status = kUWBSTATUS_FAILED;
    }
    phOsalUwb_SetMemory(pCtx, 0, sizeof(uwb_uwbs_tml_ctx_t));
    return status;
}

UWBStatus_t uwb_uwbs_tml_data_tx(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen)
{
    UWBStatus_t status;

    if (pCtx == NULL) {
        LOG_E("uwb_uwbs_tml_data_tx : uwbs tml context is NULL");
        return kUWBSTATUS_INVALID_PARAMETER;
    }

    if (pBuf == NULL || bufLen == 0) {
        LOG_E("uwb_uwbs_tml_data_tx : read buffer is Null");
        return kUWBSTATUS_INVALID_PARAMETER;
    }
    switch (pCtx->mode) {
    case kUWB_UWBS_TML_MODE_SWUP:
        status = uwb_rci_write(pCtx, pBuf, bufLen);
        break;
    case kUWB_UWBS_TML_MODE_UCI:
        status = uwb_uci_write(pCtx, pBuf, bufLen);
        break;
    default:
        LOG_E("uwb_uwbs_tml_data_tx : Invalid TML Mode");
        status = kUWBSTATUS_INVALID_PARAMETER;
        break;
    }
    return status;
}

UWBStatus_t uwb_uwbs_tml_data_rx(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen)
{
    UWBStatus_t status;

    if (pCtx == NULL) {
        LOG_E("uwb_uwbs_tml_data_rx : uwbs tml context is NULL");
        return kUWBSTATUS_INVALID_PARAMETER;
    }

    if (pBuf == NULL || pBufLen == NULL) {
        LOG_E("uwb_uwbs_tml_data_rx : read buffer is Null");
        return kUWBSTATUS_INVALID_PARAMETER;
    }
    switch (pCtx->mode) {
    case kUWB_UWBS_TML_MODE_SWUP:
        status = uwb_rci_read(pCtx, pBuf, pBufLen);
        break;
    case kUWB_UWBS_TML_MODE_UCI:
        status = uwb_uci_read(pCtx, pBuf, pBufLen);
        break;
    default:
        LOG_E("uwb_uwbs_tml_data_rx : Invalid TML Mode");
        status = kUWBSTATUS_INVALID_PARAMETER;
        break;
    }
    return status;
}

UWBStatus_t uwb_uwbs_tml_reset(uwb_uwbs_tml_ctx_t *pCtx)
{
    UWBStatus_t status        = kUWBSTATUS_FAILED;
    uint8_t resp[5]           = {0};
    size_t resp_len           = sizeof(resp);
    uint8_t count             = 0;
    uwbs_io_state_t gpioValue = kUWBS_IO_State_NA;

    uwb_bus_reset(&pCtx->busCtx);

    /*wait till IRQ pin goes low*/
    if ((uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue)) != kUWB_bus_Status_OK) {
        goto end;
    }

    while (gpioValue) {
        uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
        count++;
        if (count > TIMEOUT_DELAY) {
            goto end;
        }
    }
    status = uwb_uci_read(pCtx, resp, &resp_len);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_uwbs_tml_reset : uwb_uci_read failed");
        goto end;
    }
    if (resp_len == 0x05 && (*resp == 0x60) && (*(resp + 1) == 0x01) && (*(resp + 4) == 0x01)) {
        /* Device is ready */
        status = kUWBSTATUS_SUCCESS;
    }
    else {
        status = kUWBSTATUS_FAILED;
    }

end:
    return status;
}

void uwb_uwbs_tml_flush_read_buffer(uwb_uwbs_tml_ctx_t *pCtx)
{
    uint8_t resp[256];
    /* Flush read buffer */
    phOsalUwb_LockMutex(pCtx->mSyncMutex);
    phOsalUwb_Delay(1);
    if ((uwb_bus_data_rx(&pCtx->busCtx, resp, sizeof(resp))) != kUWB_bus_Status_OK) {
        LOG_E("Error: uwb_uwbs_tml_flush_read_buffer(), Not able to read uwb_bus_data_rx");
    }
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
}

#endif // UWBIOT_UWBD_SR040
